
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       fms.c
  * @author     baiyang
  * @date       2021-8-12
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "fms.h"
#include "logger.h"

#include <common/microbee.h>

#include <rtthread.h>
#include <rtdevice.h>
#include <rthw.h>

#include <notify/notify.h>
#include <perf/perf_counter.h>
#include <srv_channel/srv_channel.h>
#include <board_config/board_config.h>
#include <srvrly_events/srvrly_events.h>
#include <mb_follow/mb_follow.h>
/*-----------------------------------macro------------------------------------*/
#define FMS_EVENT_LOOP        (1<<0)
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
#if defined(HAL_NO_CCMRAM)
static char thread_fms_stack[1024*8];
#else
static char thread_fms_stack[1024*8] RT_SECTION(".ccmram");
#endif

struct rt_thread thread_fms_handle;

static struct rt_timer fms_timer;
static struct rt_event fms_event;

Fms_handle fms;

static AttitudeMulti_ctrl _atc;
static Position_ctrl _psc;
static struct mb_motors_matrix _motor;
static mc_loiter _loiter_nav;
static mc_wpnav wp_nav;

static const int8_t _failsafe_priorities[] = {(int8_t)FAILSAFE_ACTION_TERMINATE,
                                              (int8_t)FAILSAFE_ACTION_LAND,
                                              (int8_t)FAILSAFE_ACTION_RTL,
                                              (int8_t)FAILSAFE_ACTION_SMARTRTL_LAND,
                                              (int8_t)FAILSAFE_ACTION_SMARTRTL,
                                              (int8_t)FAILSAFE_ACTION_NONE,
                                              -1 // the priority list must end with a sentinel of -1
                                             };

perf_counter_t _fms_elapsed_perf;
perf_counter_t _fms_interval_perf;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       飞行管理模块构造函数
  * @param[in]   
  * @param[out]  
  * @retval      
  * @note        构造函数中函数的调用顺序不要随意更改
  */
void fms_ctor()
{
    fms_assign_param();

    _fms_elapsed_perf = perf_alloc(PC_ELAPSED, "fms: ET");
    _fms_interval_perf = perf_alloc(PC_INTERVAL, "fms: INR");

    fms_ahrs_init();

    // initialise rangefinder
    fms_init_rangefinder();

    fms.G_Dt = PERIOD_MS_250HZ * 0.001f;
    fms.force_flying = false;
    lpf_set_cutoff1(&fms.yaw_I_filt, 0.05f);

    fms.prev_control_mode = STABILIZE;
    fms._last_reason = MODE_REASON_UNKNOWN;
    fms.control_mode_reason = MODE_REASON_UNKNOWN;

    fms.flightmode = (mode_base_t)&fms.mode_stabilize;

    mb_arming_init();

    fms_init_rc_in();

    // initialise surface to be tracked in SurfaceTracking
    // must be before rc init to not override initial switch position
    surftrack_init(&fms.surface_tracking, (enum SurfaceType)fms.g.surftrak_mode);

    fms.ahrs = get_ahrs_view();
    fms.motors = (mb_motors_t )&_motor;
    fms.attitude_control = (mc_attctrl_t)&_atc;
    fms.pos_control = &_psc;
    fms.loiter_nav = &_loiter_nav;
    fms.wp_nav = &wp_nav;

    lpf_set_cutoff1_vec3f(&fms.land_accel_ef_filter, 1.0f);

    mb_motors_matrix_ctor((mb_motors_mat_t)fms.motors, NULL, NULL, NULL, fms.g.rc_speed);
    mb_motors_set_dt(fms.motors, PERIOD_MS_500HZ * 0.001f);

    // 电机模块初始化之后
    fms_init_rc_out();

    // motors initialised so parameters can be sent
    fms.ap.initialised_params = true;
    
    attctrl_multi_init(fms.attitude_control, fms.motors, fms.ahrs, PERIOD_MS_250HZ*0.001f, PERIOD_MS_500HZ*0.001f);

    posctrl_ctor(fms.pos_control, fms.ahrs, fms.motors, fms.attitude_control, PERIOD_MS_250HZ*0.001f);
    loiter_ctor(fms.loiter_nav, fms.ahrs, fms.pos_control, fms.attitude_control);
    wpnav_ctor(fms.wp_nav, fms.ahrs, fms.attitude_control, fms.pos_control);

    // 飞行模式构造函数要在赋值飞行模式指针之前调用
    mode_althold_ctor(&fms.mode_althold);
    mode_stabilize_ctor(&fms.mode_stabilize);
    mode_loiter_ctor(&fms.mode_loiter);
    mode_land_ctor(&fms.mode_land);
    mode_rtl_ctor(&fms.mode_rtl);
    mode_guided_ctor(&fms.mode_guided);
    mode_auto_ctor(&fms.mode_auto);
    mode_throw_ctor(&fms.mode_throw);
    mode_follow_ctor(&fms.mode_follow);

    fms_gcs_init();

    battmonitor_ctor(MASK_LOG_CURRENT, fms_handle_battery_failsafe, _failsafe_priorities);
    battmonitor_init();

    mb_follow_ctor();

    // set landed flags
    fms_set_land_complete(true);
    fms_set_land_complete_maybe(true);

    fms_actuator_armed_notify(false);

    aplog_set_vehicle_startup_writer(aplog_write_vehicle_startup_messages);

    // flag that initialisation has completed
    fms.ap.initialised = true;
}

/**
  * @brief       1Hz循环
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static inline void fms_one_hz_loop()
{
    mb_arming_update();

    if (!fms.motors->_armed) {
        fms_update_using_interlock();

        mb_motors_set_frame_class_and_type(fms.motors,
            (enum mb_motor_frame_class)fms.frame_class,
            (enum mb_motor_frame_type)fms.frame_type);

        // set all throttle channel settings
        mb_motors_update_throttle_range(fms.motors);
    }

    srv_channels_enable_aux_servos();

    notify_flags.flying = !fms.ap.land_complete;
}

/**
  * @brief       2Hz循环
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static inline void fms_two_hz_loop()
{
    // check if we've lost contact with the ground station
    fms_failsafe_gcs_check();

    // check if we've lost terrain data
    fms_failsafe_terrain_check();
}

/**
  * @brief       5Hz循环
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static inline void fms_fives_hz_loop()
{
    fms_publish_vehicle_hearbeat();
    aplog_write_vehicle_gndeffect();
}

/**
  * @brief       10Hz循环
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static inline void fms_ten_hz_loop()
{
    srv_hal_safety_update();
    rcs_read_aux_all();
    fms_lost_vehicle_check();
    fms_arm_motors_check();
    fms_auto_disarm_check();
    battmonitor_read();

    fms_publish_vehicle_vfr_hud();

    fms_check_vibration();

    aplog_write_control_tuning();
}

/**
  * @brief       20Hz循环
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static inline void fms_twenty_hz_loop()
{
    fms_read_rangefinder();
}

/**
  * @brief       50Hz循环
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static inline void fms_fifty_hz_loop()
{
    // update throttle_low_comp value (controls priority of throttle vs attitude control)
    fms_update_throttle_mix();

    // check auto_armed status
    fms_update_auto_armed();

    notify_update();

    // compensate for ground effect (if enabled)
    fms_update_ground_effect_detector();

    srvrly_update_events();
}

/**
  * @brief       100Hz循环
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static inline void fms_one_hundred_hz_loop()
{
    fms_read_radio();
    rcs_read_mode_switch();

    fms_update_throttle_hover();
    fms_standby_update();
}

/**
  * @brief       200Hz循环
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static inline void fms_two_hundred_hz_loop()
{
    fms_gcs_update();
    fms_update_land_and_crash_detectors();
}

/**
  * @brief       250Hz循环
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static inline void fms_two_hundred_and_fifty_hz_loop()
{
    fms_update_rangefinder_terrain_offset();
    fms_update_flight_mode();
}

/**
  * @brief       500Hz循环
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        用于更新需要最先更新的数据
  */
static inline void fms_fast_loop()
{
    fms_update_ahrs();

    attctrl_rate_controller_run(fms.attitude_control);

    fms_motors_output();
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static inline void fms_loop()
{
    perf_begin(_fms_elapsed_perf);
    perf_count(_fms_interval_perf);

    uint32_t NowMs = time_millis();

    TIMETAG_CHECK_EXECUTE2(fast,PERIOD_MS_500HZ,NowMs,fms_fast_loop();)

    TIMETAG_CHECK_EXECUTE2(100Hz,PERIOD_MS_100HZ,NowMs,fms_one_hundred_hz_loop();)
    TIMETAG_CHECK_EXECUTE2(250Hz,PERIOD_MS_250HZ,NowMs,fms_two_hundred_and_fifty_hz_loop();)
    TIMETAG_CHECK_EXECUTE2(200Hz,PERIOD_MS_200HZ,NowMs,fms_two_hundred_hz_loop();)

    TIMETAG_CHECK_EXECUTE2(50Hz,PERIOD_MS_50HZ,NowMs,fms_fifty_hz_loop();)
    TIMETAG_CHECK_EXECUTE2(25Hz,PERIOD_MS_20HZ,NowMs,fms_twenty_hz_loop();)
    TIMETAG_CHECK_EXECUTE2(10Hz,PERIOD_MS_10HZ,NowMs,fms_ten_hz_loop();)
    TIMETAG_CHECK_EXECUTE2(5Hz,PERIOD_MS_5HZ,NowMs,fms_fives_hz_loop();)
    TIMETAG_CHECK_EXECUTE2(2Hz,PERIOD_MS_2HZ,NowMs,fms_two_hz_loop();)
    TIMETAG_CHECK_EXECUTE2(1Hz,PERIOD_MS_1HZ,NowMs,fms_one_hz_loop();)

    perf_end(_fms_elapsed_perf);
}

/**
  * @brief       定时器回调函数，发送定时器事件
  * @param[in]   parameter  
  * @param[out]  
  * @retval      
  * @note        
  */
static void fms_time_update(void* parameter)
{
    rt_event_send(&fms_event, FMS_EVENT_LOOP);
}

/**
  * @brief       飞行任务线程入口函数
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static void fms_entry(void *parameter)
{
    rt_err_t res;
    rt_uint32_t recv_set = 0;
    rt_uint32_t wait_set = FMS_EVENT_LOOP;

    /* create event */
    res = rt_event_init(&fms_event, "fms", RT_IPC_FLAG_FIFO);
    
    /* register timer event */
    rt_timer_init(&fms_timer, "fms",
                    fms_time_update,
                    RT_NULL,
                    rt_tick_from_millisecond(1),
                    RT_TIMER_FLAG_PERIODIC | RT_TIMER_FLAG_HARD_TIMER);
    rt_timer_start(&fms_timer);

    // 电调校准,放在最后
    fms_esc_calibration_startup_check();

    while(1)
    {
        res = rt_event_recv(&fms_event, wait_set, RT_EVENT_FLAG_OR | RT_EVENT_FLAG_CLEAR, 
                                RT_WAITING_FOREVER, &recv_set);
        
        if(res == RT_EOK){
            if(recv_set & FMS_EVENT_LOOP){
                fms_loop();
            }
        }
    }
}

gp_err task_fms_init(void)
{
    rt_err_t res;

    fms_ctor();

    res = rt_thread_init(&thread_fms_handle,
                           "fms",
                           fms_entry,
                           RT_NULL,
                           &thread_fms_stack[0],
                           sizeof(thread_fms_stack),PRIORITY_FMS,5);

    RT_ASSERT(res == RT_EOK);

    if (res == RT_EOK) {
        rt_thread_startup(&thread_fms_handle);
    }

    brd_set_vehicle_init_stage(INIT_STAGE_FMS);

    return res;
}

/*------------------------------------test------------------------------------*/


